﻿
#include "BoundingSphere.h"

#include <Riccsson/Xna/Framework/BoundingBox.h>
#include <Riccsson/Xna/Framework/BoundingFrustum.h>
#include <Riccsson/Xna/Framework/Ray.h>

using namespace Riccsson::System;
using namespace Riccsson::Xna::Framework;

BoundingSphere::BoundingSphere()
{
	this->Center = Vector3(0,0,0);
	this->Radius = 1;
}
BoundingSphere::BoundingSphere(Vector3 center, float radius)
{
	this->Center = center;
	this->Radius = radius;
}
BoundingSphere BoundingSphere::Transform(Matrix matrix)
{
	BoundingSphere sphere;
	sphere.Center = Vector3::Transform(this->Center, matrix);
	sphere.Radius = this->Radius * ((float)Riccsson::System::Math::Sqrt((double)Riccsson::System::Math::Max(((matrix.M11 * matrix.M11) + (matrix.M12 * matrix.M12)) + (matrix.M13 * matrix.M13), Riccsson::System::Math::Max(((matrix.M21 * matrix.M21) + (matrix.M22 * matrix.M22)) + (matrix.M23 * matrix.M23), ((matrix.M31 * matrix.M31) + (matrix.M32 * matrix.M32)) + (matrix.M33 * matrix.M33)))));
	return sphere;
}
void BoundingSphere::Transform(Matrix ref matrix, BoundingSphere out result)
{
	result.Center = Vector3::Transform(this->Center, matrix);
	result.Radius = this->Radius * ((float)Riccsson::System::Math::Sqrt((double)Riccsson::System::Math::Max(((matrix.M11 * matrix.M11) + (matrix.M12 * matrix.M12)) + (matrix.M13 * matrix.M13), Riccsson::System::Math::Max(((matrix.M21 * matrix.M21) + (matrix.M22 * matrix.M22)) + (matrix.M23 * matrix.M23), ((matrix.M31 * matrix.M31) + (matrix.M32 * matrix.M32)) + (matrix.M33 * matrix.M33)))));
}
ContainmentType BoundingSphere::Contains(BoundingBox& box)
{
	//check if all corner is in sphere
	bool inside = true;
	foreach2 (var, corner, box.GetCorners())
	{
		if (this->Contains(*corner) == ContainmentType::Disjoint)
		{
			inside = false;
			break;
		}
	}

	if (inside)
		return ContainmentType::Contains;

	//check if the distance from sphere center to cube face < radius
	double dmin = 0;

	if (Center.X < box.Min.X)
		dmin += (Center.X - box.Min.X) * (Center.X - box.Min.X);

	else if (Center.X > box.Max.X)
			dmin += (Center.X - box.Max.X) * (Center.X - box.Max.X);

	if (Center.Y < box.Min.Y)
		dmin += (Center.Y - box.Min.Y) * (Center.Y - box.Min.Y);

	else if (Center.Y > box.Max.Y)
		dmin += (Center.Y - box.Max.Y) * (Center.Y - box.Max.Y);

	if (Center.Z < box.Min.Z)
		dmin += (Center.Z - box.Min.Z) * (Center.Z - box.Min.Z);

	else if (Center.Z > box.Max.Z)
		dmin += (Center.Z - box.Max.Z) * (Center.Z - box.Max.Z);

	if (dmin <= Radius * Radius) 
		return ContainmentType::Intersects;
            
	//else disjoint
	return ContainmentType::Disjoint;

}
void BoundingSphere::Contains(BoundingBox ref box, ContainmentType out result)
{
	result = this->Contains(box);
}
ContainmentType BoundingSphere::Contains(BoundingFrustum& frustum)
{
	//check if all corner is in sphere
	bool inside = true;

	GC_PTR<Riccsson::System::Array<Vector3>> corners = frustum.GetCorners();
	foreach2 (var, corner, corners)
	{
		if (this->Contains(*corner) == ContainmentType::Disjoint)
		{
			inside = false;
			break;
		}
	}
	if (inside)
		return ContainmentType::Contains;

	//check if the distance from sphere center to frustrum face < radius
	double dmin = 0;
	//TODO : calcul dmin

	if (dmin <= Radius * Radius)
		return ContainmentType::Intersects;

	//else disjoint
	return ContainmentType::Disjoint;
}
ContainmentType BoundingSphere::Contains(BoundingSphere sphere)
{
	float val = Vector3::Distance(sphere.Center, Center);

	if (val > sphere.Radius + Radius)
		return ContainmentType::Disjoint;

	else if (val <= Radius - sphere.Radius)
		return ContainmentType::Contains;

	else
		return ContainmentType::Intersects;
}
void BoundingSphere::Contains(BoundingSphere ref sphere, ContainmentType out result)
{
	result = Contains(sphere);
}
ContainmentType BoundingSphere::Contains(Vector3 point)
{
	float distance = Vector3::Distance(point, Center);

	if (distance > this->Radius)
		return ContainmentType::Disjoint;

	else if (distance < this->Radius)
		return ContainmentType::Contains;

	return ContainmentType::Intersects;
}
void BoundingSphere::Contains(Vector3 ref point, ContainmentType out result)
{
	result = Contains(point);
}
BoundingSphere BoundingSphere::CreateFromBoundingBox(BoundingBox& box)
{
	// Find the center of the box.
	Vector3 center = Vector3((box.Min.X + box.Max.X) / 2.0f,
							(box.Min.Y + box.Max.Y) / 2.0f,
							(box.Min.Z + box.Max.Z) / 2.0f);

	// Find the distance between the center and one of the corners of the box.
	float radius = Vector3::Distance(center, box.Max);

	return BoundingSphere(center, radius);
}
void BoundingSphere::CreateFromBoundingBox(BoundingBox ref box, BoundingSphere out result)
{
	result = CreateFromBoundingBox(box);
}
BoundingSphere BoundingSphere::CreateFromFrustum(BoundingFrustum& frustum)
{
	return BoundingSphere::CreateFromPoints(frustum.GetCorners());
}
BoundingSphere BoundingSphere::CreateFromPoints(Riccsson::System::Collections::Generic::IEnumerable<Vector3>* points)
{
	if (points == null)
		throw Riccsson::System::ArgumentNullException("points");

	float radius = 0;
	Vector3 center;
	// First, we'll find the center of gravity for the point 'cloud'.
	int num_points = 0; // The number of points (there MUST be a better way to get this instead of counting the number of points one by one?)
            
	foreach2 (var, v, points)
	{
		center += *v;    // If we actually knew the number of points, we'd get better accuracy by adding v / num_points.
		++num_points;
	}
            
	center /= (float)num_points;

	// Calculate the radius of the needed sphere (it equals the distance between the center and the point further away).
	foreach2 (var, v, points)
	{
		float distance = ((Vector3)(*v - center)).Length();
                
		if (distance > radius)
			radius = distance;
	}

	return BoundingSphere(center, radius);
}
BoundingSphere BoundingSphere::CreateMerged(BoundingSphere original, BoundingSphere additional)
{
	Vector3 ocenterToaCenter = Vector3::Subtract(additional.Center, original.Center);
	float distance = ocenterToaCenter.Length();
	if (distance <= original.Radius + additional.Radius)//intersect
	{
		if (distance <= original.Radius - additional.Radius)//original contain additional
			return original;
		if (distance <= additional.Radius - original.Radius)//additional contain original
			return additional;
	}

	//else find center of new sphere and radius
	float leftRadius = Riccsson::System::Math::Max(original.Radius - distance, additional.Radius);
	float Rightradius = Riccsson::System::Math::Max(original.Radius + distance, additional.Radius);
	ocenterToaCenter = ocenterToaCenter + (((leftRadius - Rightradius) / (2 * ocenterToaCenter.Length())) * ocenterToaCenter);//oCenterToResultCenter
            
	BoundingSphere result;
	result.Center = original.Center + ocenterToaCenter;
	result.Radius = (leftRadius + Rightradius) / 2;
	return result;
}
void BoundingSphere::CreateMerged(BoundingSphere ref original, BoundingSphere ref additional, BoundingSphere out result)
{
	result = BoundingSphere::CreateMerged(original, additional);
}
bool BoundingSphere::Equals(BoundingSphere* other)
{
	return this->Center == other->Center && this->Radius == other->Radius;
}
bool BoundingSphere::Equals(Riccsson::System::object* obj)
{
	if (instanceof(obj, BoundingSphere))
		return this->Equals((BoundingSphere*)obj);

	return false;
}
int BoundingSphere::GetHashCode()
{
	//return this->Center.GetHashCode() + this->Radius.GetHashCode();
	throw;
}
bool BoundingSphere::Intersects(BoundingBox& box)
{
	return box.Intersects(*this);
}
void BoundingSphere::Intersects(BoundingBox ref box, bool out result)
{
	result = Intersects(box);
}
bool BoundingSphere::Intersects(BoundingFrustum& frustum)
{
	// ???
	//if (frustum == null)
	//	throw Riccsson::System::NullReferenceException();

	throw Riccsson::System::NotImplementedException();
}
bool BoundingSphere::Intersects(BoundingSphere sphere)
{
	float val = Vector3::Distance(sphere.Center, Center);
	if (val > sphere.Radius + Radius)
		return false;
	return true;
}
void BoundingSphere::Intersects(BoundingSphere ref sphere, bool out result)
{
	result = Intersects(sphere);
}
PlaneIntersectionType BoundingSphere::Intersects(Plane plane)
{
	//var result = default(PlaneIntersectionType);
	//// TODO: we might want to inline this for performance reasons
	//this->Intersects(ref plane, out result);
	//return result;
	throw;
}
void BoundingSphere::Intersects(Plane ref plane, PlaneIntersectionType out result)
{
	//var distance = default(float);
	//// TODO: we might want to inline this for performance reasons
	//Vector3::Dot(ref plane.Normal, ref this->Center, out distance);
	//distance += plane.D;
	//if (distance > this->Radius)
	//	result = PlaneIntersectionType.Front;
	//else if (distance < -this->Radius)
	//	result = PlaneIntersectionType.Back;
	//else
	//	result = PlaneIntersectionType.Intersecting;
	throw;
}
Riccsson::System::Nullable<float> BoundingSphere::Intersects(Ray& ray)
{
	return ray.Intersects(*this);
}
void BoundingSphere::Intersects(Ray ref ray, Riccsson::System::Nullable<float> out result)
{
	result = Intersects(ray);
}
GC_PTR<Riccsson::System::string> BoundingSphere::ToString()
{
	//return string::Format(CultureInfo::CurrentCulture, "{{Center:{0} Radius:{1}}}", this->Center.ToString(), this->Radius.ToString());
	throw;
}